//
// Created by rebeater on 2020/10/7.
//
#include "global_defines.h"
#include "config.h"



/**
 * 缺省imu参数:
 * */
#if IMU_DEVICE == IMU_ADIS16460
ImuPara default_imu_para = {
	.arw = 0.112 * _deg / _sqrt_h, .vrw= 0.0025 / _sqrt_h,
	.ab_ini = {1000 * _mGal, -1500 * _mGal, -3000 * _mGal},
	.gb_ini={0 * _deg / _hour, -0 * _deg / _hour, -0 * _deg / _hour},
	.as_ini = {0, 0, 0},
	.gs_ini={0, 0, 0},
	.ab_std = 400 * _mGal,
	.gb_std =  4.83 * _deg / _hour,
	.gs_std= 1000 * _ppm,
	.as_std = 1000 * _ppm,
	.at_corr = _hour,
	.gt_corr=  1 * _hour
};
int imu_rate = 128;
#elif IMU_DEVICE == IMU_KYIMU102
ImuPara default_imu_para = {
	.arw = 0.112 * _deg / _sqrt_h, .vrw= 0.0025 / _sqrt_h,
	.ab_ini = {1000 * _mGal, -1500 * _mGal, -3000 * _mGal},
	.gb_ini={0 * _deg / _hour, -0 * _deg / _hour, -0 * _deg / _hour},
	.as_ini = {0, 0, 0},
	.gs_ini={0, 0, 0},
	.ab_std = 400 * _mGal,
	.gb_std =  4.83 * _deg / _hour,
	.gs_std= 1000 * _ppm,
	.as_std = 1000 * _ppm,
	.at_corr = 1 * _hour,
	.gt_corr=  1 * _hour
};
int imu_rate = 200;
#elif IMU_DEVICE == IMU_STIM300
/*TODO STIM300 default parameters*/
#elif IMU_DEVICE == IMU_ICM20602
ImuPara default_imu_para = {
	0.66 * _deg / _sqrt_h, 0.24 / _sqrt_h,
	{0 * _mGal, 0 * _mGal, 0 * _mGal},
	{0 * _deg / _hour, -0 * _deg / _hour, -0 * _deg / _hour},
	{0, 0, 0},
	{0, 0, 0},
	1500 * _mGal,
	480 * _deg / _hour,
	1000 * _ppm,
	1000 * _ppm,
	1 * _hour,
	1 * _hour
};
int imu_rate = 50;
#else
#error "IMU_DEVICE is NOT invalid,please check if IMU_DEVICE is supported!"
#endif

/**
 * 缺省初始状态 瞎给的
 * */
/**]
 *  39° 58.826'北
 */
NavPva initial_epoch{
	0,
	39.980597981762785 * _deg,
	116.320376689788375 * _deg,
	40,
	0,
	0,
	0,
	0 * _deg,
	0 * _deg,
	0 * _deg,
	{-0 * _mGal, -0 * _mGal, -0 * _mGal},
	{-0 * _deg / _hour, 0 * _deg / _hour, -0 * _deg / _hour},
	/* {1, 1, 1},
	 {1.0, 1.0, 1.0},
	 {2.0, 2.0, 5.0},*/
	{1, 0}
};

/**
 * 缺省配置
 * */
Option default_opt = {
	.imuPara = default_imu_para,
	.imu_data_rate= imu_rate,
	.align_mode= Moving,
	.nhc_enable=1, .nhc_var=  0.01,
	.zupt_enable = 1, .zupt_var =  0.01, .zupta_var= 0.01,
	.antenna_arm= {0.2, 1, 0.3},
	.odo_enable = 1,
	.odo_var = 0.1,
	.odo_kd = 1.00,
	.odo_wheel_radius = 0.35,
};


